/*
 * Copyright (C) 2020 1km Group Holding Limited
 */

#include "dm556_driver.h"
#include "ulog/ulog.h"

#define DM556_STATE_INDEX 0x6041
#define DM556_STATE_SUB_INDEX 0x00

#define DM556_CONTROL_INDEX 0x6040
#define DM556_CONTROL_SUB_INDEX 0x00

#define DM556_OPERATE_MODE_INDEX 0x6060
#define DM556_OPERATE_MODE_SUB_INDEX 0x00

#define DM556_DISPLAY_OPERATE_MODE_INDEX 0x6061
#define DM556_DISPLAY_OPERATE_MODE_SUB_INDEX 0x00

#define DM556_INPUT_FUNC_INDEX 0x2152
#define DM556_IN1_FUNC_SUB_INDEX 0x01
#define DM556_IN2_FUNC_SUB_INDEX 0x02
#define DM556_IN3_FUNC_SUB_INDEX 0x03

#define DM556_INPUT_FILTER_TIME_INDEX 0x2153
#define DM556_IN1_FILTER_TIME_SUB_INDEX 0x01
#define DM556_IN2_FILTER_TIME_SUB_INDEX 0x02
#define DM556_IN3_FILTER_TIME_SUB_INDEX 0x03

#define DM556_INPUT_POLARITY_CONFIG_INDEX 0x2154
#define DM556_INPUT_POLARITY_CONFIG_SUB_INDEX 0x00

#define DM556_INPUT_FUNC_STATE_INDEX 0x60FD
#define DM556_INPUT_FUNC_STATE_SUB_INDEX 0x00

#define DM556_INPUT_PIN_STATE_INDEX 0x2155
#define DM556_INPUT_PIN_STATE_SUB_INDEX 0x00

#define DM556_OUTPUT_FUNC_INDEX 0x2005
#define DM556_OUT1_FUNC_SUB_INDEX 0x01
#define DM556_OUT2_FUNC_SUB_INDEX 0x02

#define DM556_OUTPUT_RESISTANCE_INDEX 0x2008
#define DM556_OUTPUT_RESISTANCE_SUB_INDEX 0x00

#define DM556_OUTPUT_SET_INDEX 0x60FE
#define DM556_OUTPUT_OUTPUT_SUB_INDEX 0x01
#define DM556_OUTPUT_ENABLE_SUB_INDEX 0x02

#define DM556_SPIN_DIRECTION_INDEX 0x2051
#define DM556_SPIN_DIRECTION_SUB_INDEX 0x00

#define DM556_DECELERATIONN_INDEX 0x6084
#define DM556_DECELERATIONN_SUB_INDEX 0x00

#define DM556_EMERGENCY_STOP_DECELERATIONN_INDEX 0x6085
#define DM556_EMERGENCY_STOP_DECELERATIONN_SUB_INDEX 0x00

#define DM556_MOTOR_RESOLUTION_INDEX 0x2001
#define DM556_MOTOR_RESOLUTION_SUB_INDEX 0x00

#define DM556_PARAM_SAVE_INDEX 0x1010
#define DM556_ALL_PARAM_SAVE_SUB_INDEX 0x01
#define DM556_COMM_PARAM_SAVE_SUB_INDEX 0x02
#define DM556_APP_PARAM_SAVE_SUB_INDEX 0x03
#define DM556_MANUFACTURER_PARAM_SAVE_SUB_INDEX 0x04

#define DM556_PARAM_RESTORE_INDEX 0x1011
#define DM556_ALL_PARAM_RESTORE_SUB_INDEX 0x01
#define DM556_COMM_PARAM_RESTORE_SUB_INDEX 0x02
#define DM556_APP_PARAM_RESTORE_SUB_INDEX 0x03
#define DM556_MANUFACTURER_PARAM_RESTORE_SUB_INDEX 0x04

#define DM556_TARGET_POSITION_INDEX 0x607a
#define DM556_TARGET_POSITION_SUB_INDEX 0x00

#define DM556_PROFILE_VELOCITY_INDEX 0x6081
#define DM556_PROFILE_VELOCITY_SUB_INDEX 0x00

#define DM556_END_VELOCITY_INDEX 0x6082
#define DM556_END_VELOCITY_SUB_INDEX 0x00

#define DM556_PROFILE_ACCELERATION_INDEX 0x6083
#define DM556_PROFILE_ACCELERATION_SUB_INDEX 0x00

#define DM556_PROFILE_DECELERATION_INDEX 0x6084
#define DM556_PROFILE_DECELERATION_SUB_INDEX 0x00

#define DM556_TARGET_VELOCITY_INDEX 0x60ff
#define DM556_TARGET_VELOCITY_SUB_INDEX 0x00

#define DM556_HOMING_METHOD_INDEX 0x6098
#define DM556_HOMING_METHOD_SUB_INDEX 0x00

#define DM556_HOMING_SPEEDS_INDEX 0x6099
#define DM556_HOMING_FAST_SPEEDS_SUB_INDEX 0x01
#define DM556_HOMING_SLOW_SPEEDS_SUB_INDEX 0x02

#define DM556_HOMING_ACCELERATION_INDEX 0x609A
#define DM556_HOMING_ACCELERATION_SUB_INDEX 0x00

#define DM556_HOMING_OFFSET_INDEX 0x607c
#define DM556_HOMING_OFFSET_SUB_INDEX 0x00

/**
 * @brief 保存出厂参数
 * 
 * @param node_id CAN端口与从站节点号
 * @return int32_t 0成功，非0表示失败
 */
int32_t save_manufacturer_param(const can_node_id_t *node_id)
{
	uint32_t save_param_cmd = 0x65766173;
	int32_t rc = 0;
			
	rc = co_sdo_write_od(node_id, DM556_PARAM_SAVE_INDEX, DM556_MANUFACTURER_PARAM_SAVE_SUB_INDEX, &save_param_cmd, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_PARAM_SAVE_INDEX, DM556_MANUFACTURER_PARAM_SAVE_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 恢复出厂参数
 * 
 * @param node_id CAN端口与从站节点号
 * @return int32_t 0成功，非0表示失败
 */
int32_t restore_manufacturer_param(const can_node_id_t *node_id)
{
	uint32_t restore_param_cmd = 0x64616f6c;
	int32_t rc = 0;
			
	rc = co_sdo_write_od(node_id, DM556_PARAM_RESTORE_INDEX, DM556_MANUFACTURER_PARAM_RESTORE_SUB_INDEX, &restore_param_cmd, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_PARAM_RESTORE_INDEX, DM556_MANUFACTURER_PARAM_RESTORE_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置电机旋转反向
 * 
 * @param node_id CAN端口与从站节点号
 * @param dir 0与指令反向一致，1与指令方向相反
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_spin_direction(const can_node_id_t *node_id, uint32_t dir)
{
	int32_t rc = 0;
		
	rc = co_sdo_write_od(node_id, DM556_SPIN_DIRECTION_INDEX, DM556_SPIN_DIRECTION_SUB_INDEX, &dir, 2);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_SPIN_DIRECTION_INDEX, DM556_SPIN_DIRECTION_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置电机减速停止
 * 
 * @param node_id CAN端口与从站节点号
 * @param deceleration 减速度，设置为0时电机无法减速停止
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_deceleration(const can_node_id_t *node_id, uint32_t deceleration)
{
	int32_t rc = 0;
		
	rc = co_sdo_write_od(node_id, DM556_DECELERATIONN_INDEX, DM556_DECELERATIONN_SUB_INDEX, &deceleration, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_DECELERATIONN_INDEX, DM556_DECELERATIONN_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置电机急停减速度
 * 
 * @param node_id CAN端口与从站节点号
 * @param es_deceleration 设置不为0时，电机在触发急停时，电机按照此减速度进行 减速停止， 设置为0时，表示电机立即停止。
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_emergency_stop_deceleration(const can_node_id_t *node_id, uint32_t es_deceleration)
{
	int32_t rc = 0;
		
	rc = co_sdo_write_od(node_id, DM556_EMERGENCY_STOP_DECELERATIONN_INDEX, DM556_EMERGENCY_STOP_DECELERATIONN_SUB_INDEX, &es_deceleration, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_EMERGENCY_STOP_DECELERATIONN_INDEX, DM556_EMERGENCY_STOP_DECELERATIONN_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置电机细分
 * 
 * @param node_id CAN端口与从站节点号
 * @param val 细分值
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_motor_resolution(const can_node_id_t *node_id, uint16_t val)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_MOTOR_RESOLUTION_INDEX, DM556_MOTOR_RESOLUTION_SUB_INDEX, &val, 2);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_MOTOR_RESOLUTION_INDEX, DM556_MOTOR_RESOLUTION_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置外部输入口INn功能
 * 
 * @param node_id CAN端口与从站节点号
 * @param input_num 输入口
 * @param func 1：原点信号 2：正限位 4：负限位 8：快速停止
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_input_func(const can_node_id_t *node_id, int8_t input_num, uint32_t func)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_INPUT_FUNC_INDEX, input_num, &func, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_INPUT_FUNC_INDEX, input_num);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置外部输入口INn滤波时间
 * 
 * @param node_id CAN端口与从站节点号
 * @param input_num 输入口
 * @param filter_time 滤波时间
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_input_filter_time(const can_node_id_t *node_id, int8_t input_num, uint32_t filter_time)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_INPUT_FILTER_TIME_INDEX, input_num, &filter_time, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_INPUT_FILTER_TIME_INDEX, input_num);
		return -1;
	}

	return 0;
}

/**
 * @brief 外部输入极性配置
 * 
 * @param node_id CAN端口与从站节点号
 * @param polarity_config Bit0：对应驱动器外部 1 号输入口 IN1 Bit1：对应驱动器外部 1 号输入口 IN2 Bit2：对应驱动器外部 1 号输入口 
 * 						  IN3 Bit3：对应驱动器外部 1 号输入口 IN4
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_input_polarity_config(const can_node_id_t *node_id, uint16_t polarity_config)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_INPUT_POLARITY_CONFIG_INDEX, DM556_INPUT_POLARITY_CONFIG_SUB_INDEX, &polarity_config, 2);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_INPUT_POLARITY_CONFIG_INDEX, DM556_INPUT_POLARITY_CONFIG_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置外部输入功能状态
 * 
 * @param node_id CAN端口与从站节点号
 * @param input_func_state bit0：负限位当前的状态 bit1：正限位当前的状态 bit2：原点信号当前的状态 bit16：快速停止信号当前的状态 
 * 						   注意：DMA882-CAN 有 4 个输入口，才有此信号) Bit17~bit21 对应 IN1~IN5 自定义功能时的输入电平
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_input_func_state(const can_node_id_t *node_id, uint32_t input_func_state)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_INPUT_FUNC_STATE_INDEX, DM556_INPUT_FUNC_STATE_SUB_INDEX, &input_func_state, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_INPUT_FUNC_STATE_INDEX, DM556_INPUT_FUNC_STATE_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置外部输入引脚状态
 * 
 * @param node_id CAN端口与从站节点号
 * @param input_pin_state bit0：对应外部 IN1 当前的状态 bit1：对应外部 IN2 当前的状态 bit2：对应外部 IN3 当前的状态 
 * 						  Bit3：对应外部 IN4 当前的状态 （注意：DMA882-CAN 有 4 个输入口，才有此信号)
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_input_pin_state(const can_node_id_t *node_id, uint16_t input_pin_state)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_INPUT_PIN_STATE_INDEX, DM556_INPUT_PIN_STATE_SUB_INDEX, &input_pin_state, 2);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_INPUT_PIN_STATE_INDEX, DM556_INPUT_PIN_STATE_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置外部输出口OUTn功能
 * 
 * @param node_id CAN端口与从站节点号
 * @param output_num 输出口
 * @param func Bit0：报警输出 Bit2：到位输出 Bit4：主站控制输出，当设置为此状态时，主站对 60FE 操 作控制输出口的状态。
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_output_func(const can_node_id_t *node_id, int8_t output_num, uint32_t func)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_OUTPUT_FUNC_INDEX, output_num, &func, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_OUTPUT_FUNC_INDEX, output_num);
		return -1;
	}

	return 0;
}

/**
 * @brief 外部输出口阻态设置
 * 
 * @param node_id CAN端口与从站节点号
 * @param output_resistance 此参数设置外部输出组态，0：正逻辑；1：反逻辑 Bit0：对应驱动器外部输入口1 Bit1：对应驱动器外部输入口2
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_output_resistance(const can_node_id_t *node_id, uint16_t output_resistance)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_OUTPUT_RESISTANCE_INDEX, DM556_OUTPUT_RESISTANCE_SUB_INDEX, &output_resistance, 2);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_OUTPUT_RESISTANCE_INDEX, DM556_OUTPUT_RESISTANCE_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 外部输出口使能
 * 
 * @param node_id CAN端口与从站节点号
 * @param output_enable 此参数仅仅是在 2005+01/02 的功能 Bit4 设置为 1时有效，
 * 						用来设置外部输出的使能。 Bit16：对应外部输出口 out1； Bit17：对应外部输出口 out2； 其他未定义的位保留。
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_output_enable(const can_node_id_t *node_id, uint32_t output_enable)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_OUTPUT_SET_INDEX, DM556_OUTPUT_ENABLE_SUB_INDEX, &output_enable, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_OUTPUT_SET_INDEX, DM556_OUTPUT_ENABLE_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 外部输出口输出
 * 
 * @param node_id CAN端口与从站节点号
 * @param output_output 此参数仅仅是在 2005+01/02 的功能 Bit4 设置为 1 时有效，用来设置外部输出的开启。 
 * 						Bit16：对应外部输出口 out1； Bit17：对应外部输出口 out2； 其他未定义的位保留。
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_output_output(const can_node_id_t *node_id, uint32_t output_output)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_OUTPUT_SET_INDEX, DM556_OUTPUT_OUTPUT_SUB_INDEX, &output_output, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_OUTPUT_SET_INDEX, DM556_OUTPUT_OUTPUT_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置回原点方式
 * 
 * @param node_id CAN端口与从站节点号
 * @param cmd 设置回原点方式，DM-CAN 步进系列产品支持的回零方式：17~30
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_homing_method(const can_node_id_t *node_id, int8_t cmd)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_HOMING_METHOD_INDEX, DM556_HOMING_METHOD_SUB_INDEX, &cmd, 1);
	
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_HOMING_METHOD_INDEX, DM556_HOMING_METHOD_SUB_INDEX);
		return -1;
	}
	
	return 0;
}

/**
 * @brief 设置回原点高速运行速度
 * 
 * @param node_id CAN端口与从站节点号
 * @param fast_speed 高速值
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_homing_fast_speed(const can_node_id_t *node_id, uint32_t fast_speed)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_HOMING_SPEEDS_INDEX, DM556_HOMING_FAST_SPEEDS_SUB_INDEX, &fast_speed, 4);
	
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_HOMING_SPEEDS_INDEX, DM556_HOMING_FAST_SPEEDS_SUB_INDEX);
		return -1;
	}
	
	return 0;
}

/**
 * @brief 设置回原点低速运行速度
 * 
 * @param node_id CAN端口与从站节点号
 * @param slow_speed 低速值
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_homing_slow_speed(const can_node_id_t *node_id, uint32_t slow_speed)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_HOMING_SPEEDS_INDEX, DM556_HOMING_SLOW_SPEEDS_SUB_INDEX, &slow_speed, 4);
	
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_HOMING_SPEEDS_INDEX, DM556_HOMING_SLOW_SPEEDS_SUB_INDEX);
		return -1;
	}
	
	return 0;
}

/**
 * @brief 设置回原点加/减速度。
 * 
 * @param node_id CAN端口与从站节点号
 * @param acceleration 回原点加/减速度。
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_homing_acceleration(const can_node_id_t *node_id, uint32_t acceleration)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_HOMING_ACCELERATION_INDEX, DM556_HOMING_ACCELERATION_SUB_INDEX, &acceleration, 4);
	
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_HOMING_ACCELERATION_INDEX, DM556_HOMING_ACCELERATION_SUB_INDEX);
		return -1;
	}
	
	return 0;
}

/**
 * @brief 设置回机械原点后偏移机械原点的偏移量，将此处位置作为机械零点，如果设置为 0，机械原点则和机械零 点重合。 
 * 		  原点偏移可以设置正数或者负数，表示偏离机械原点的左边或者右边。
 * 
 * @param node_id CAN端口与从站节点号
 * @param offset 原点偏移量
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_homing_offset(const can_node_id_t *node_id, int32_t offset)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_HOMING_OFFSET_INDEX, DM556_HOMING_OFFSET_SUB_INDEX, &offset, 4);
	
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_HOMING_OFFSET_INDEX, DM556_HOMING_OFFSET_SUB_INDEX);
		return -1;
	}
	
	return 0;
}

/**
 * @brief 获取DM_556驱动器的状态
 * 
 * @param node_id CAN端口与从站节点号
 * @return uint16_t 非负数表示状态值，负数表示失败
 */
uint16_t get_dm556_state(const can_node_id_t *node_id)
{	
	uint16_t sdo_read_value = 0xFFFF;
	int32_t rc = 0;

	rc = co_sdo_read_od(node_id, DM556_STATE_INDEX, DM556_STATE_SUB_INDEX, &sdo_read_value, 2);
	if (0 > rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Read on 0x%x 0x%x fail", DM556_STATE_INDEX, DM556_STATE_SUB_INDEX);
		return rc;
	} else {
			return sdo_read_value;
	}	
}

/**
 * @brief 设置DM_556驱动器的控制命令
 * 
 * @param node_id CAN端口与从站节点号
 * @param cmd 控制命令
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_dm556_control(const can_node_id_t *node_id, uint16_t cmd)
{
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, DM556_CONTROL_INDEX, DM556_CONTROL_SUB_INDEX, &cmd, 2);
	
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_CONTROL_INDEX, DM556_CONTROL_SUB_INDEX);
		return -1;
	}
	
	return 0;
}

/**
 * @brief 使能雷赛步进驱动器
 * 
 * @param node_id CAN端口与从站节点号
 * @return int32_t 0成功，非0表示失败
 */
int32_t enable_dm556(const can_node_id_t *node_id)
{
	uint16_t dm556_state;
	int32_t rc = 0;
		
	dm556_state = get_dm556_state(node_id);
	if(0x218 == (dm556_state & 0x23f)){
		return -1;	
	}
		
	rc = set_dm556_control(node_id, RESET_CMD);
	if (0 != rc) {
		return -1;
	}

	dm556_state = get_dm556_state(node_id);
	if(0x200 != (dm556_state & 0x200)){
		return -1;	
	}

	rc = set_dm556_control(node_id, DRIVER_READY_CMD);
	if (0 != rc) {
		return -1;
	}

	dm556_state = get_dm556_state(node_id);
	if(0x231 != (dm556_state & 0x231)){
		return -1;	
	}

	rc = set_dm556_control(node_id, WAIT_ENABLE_CMD);
	if (0 != rc) {
		return -1;
	}

	dm556_state = get_dm556_state(node_id);
	if(0x233 != (dm556_state & 0x233)){
		return -1;	
	}

	rc = set_dm556_control(node_id, WAIT_RUN_CMD);
	if (0 != rc) {
		return -1;
	}

	
	dm556_state = get_dm556_state(node_id);
	if(0x237 != (dm556_state & 0x237)){
		return -1;	
	}
 
	return 0;
}

/**
 * @brief 设置操作模式
 * 
 * @param node_id CAN端口与从站节点号
 * @param operating_mode 1：协议位置模式 3：协议速度模式 6：原点模式
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_operating_mode(const can_node_id_t *node_id, uint8_t operating_mode)
{
	int32_t rc = 0;

	rc = co_sdo_write_od(node_id, DM556_OPERATE_MODE_INDEX, DM556_OPERATE_MODE_SUB_INDEX, &operating_mode, 1);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_OPERATE_MODE_INDEX, DM556_OPERATE_MODE_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 获取当前操作模式
 * 
 * @param node_id CAN端口与从站节点号
 * @return uint8_t 当前操作模式
 */
uint8_t get_operating_mode(const can_node_id_t *node_id)
{
	uint8_t sdo_read_value1 = 0;
	int32_t rc = 0;
	
	rc = co_sdo_read_od(node_id, DM556_DISPLAY_OPERATE_MODE_INDEX, DM556_DISPLAY_OPERATE_MODE_SUB_INDEX, &sdo_read_value1, 1);
	if (0 > rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Read on 0x%x 0x%x fail", DM556_DISPLAY_OPERATE_MODE_INDEX, DM556_DISPLAY_OPERATE_MODE_SUB_INDEX);
		return rc;
	} else {
			return sdo_read_value1;
	}
}

/**
 * @brief 设置协议速度模式目标速度
 * 
 * @param node_id CAN端口与从站节点号
 * @param target_velocity 目标速度
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_target_velocity(const can_node_id_t *node_id, int32_t target_velocity)
{
	int32_t rc = 0;

	rc = co_sdo_write_od(node_id, DM556_TARGET_VELOCITY_INDEX, DM556_TARGET_VELOCITY_SUB_INDEX, &target_velocity, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_TARGET_VELOCITY_INDEX, DM556_TARGET_VELOCITY_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置协议位置模式下的目标位置
 * 
 * @param node_id CAN端口与从站节点号
 * @param target_position 目标位置
 * @return int32_t 0成功，非0表示失败 
 */
int32_t set_target_position(const can_node_id_t *node_id, int32_t target_position)
{
	int32_t rc = 0;

	rc = co_sdo_write_od(node_id, DM556_TARGET_POSITION_INDEX, DM556_TARGET_POSITION_SUB_INDEX, &target_position, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_TARGET_POSITION_INDEX, DM556_TARGET_POSITION_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置协议最大速度
 * 
 * @param node_id CAN端口与从站节点号
 * @param val 协议最大速度
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_profile_velocity(const can_node_id_t *node_id, uint32_t val)
{
	int32_t rc = 0;

	rc = co_sdo_write_od(node_id, DM556_PROFILE_VELOCITY_INDEX, DM556_PROFILE_VELOCITY_SUB_INDEX, &val, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_PROFILE_VELOCITY_INDEX, DM556_PROFILE_VELOCITY_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置协议启/停速度
 * 
 * @param node_id CAN端口与从站节点号
 * @param val 协议启/停速度
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_end_velocity(const can_node_id_t *node_id, uint32_t val)
{
	int32_t rc = 0;

	rc = co_sdo_write_od(node_id, DM556_END_VELOCITY_INDEX, DM556_END_VELOCITY_SUB_INDEX, &val, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_END_VELOCITY_INDEX, DM556_END_VELOCITY_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置协议加速度
 * 
 * @param node_id CAN端口与从站节点号
 * @param val 协议加速度
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_profile_acceleration(const can_node_id_t *node_id, uint32_t val)
{
	int32_t rc = 0;

	rc = co_sdo_write_od(node_id, DM556_PROFILE_ACCELERATION_INDEX, DM556_PROFILE_ACCELERATION_SUB_INDEX, &val, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_PROFILE_ACCELERATION_INDEX, DM556_PROFILE_ACCELERATION_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置协议减速度
 * 
 * @param node_id CAN端口与从站节点号
 * @param val 协议减速度
 * @return int32_t 0成功，非0表示失败
 */
int32_t set_profile_deceleration(const can_node_id_t *node_id, uint32_t val)
{
	int32_t rc = 0;

	rc = co_sdo_write_od(node_id, DM556_PROFILE_DECELERATION_INDEX, DM556_PROFILE_DECELERATION_SUB_INDEX, &val, 4);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", DM556_PROFILE_DECELERATION_INDEX, DM556_PROFILE_DECELERATION_SUB_INDEX);
		return -1;
	}

	return 0;
}

/**
 * @brief 设置相对位置移动
 * 
 * @param node_id CAN端口与从站节点号
 * @param target_position 相对位置值
 * @return int32_t 0成功，非0表示失败
 */
int32_t relative_position_move(const can_node_id_t *node_id, int32_t target_position)
{
	uint16_t dm556_state;
	int32_t rc = 0;

	rc = set_target_position(node_id, target_position);
	if (0 != rc) {
		return -1;
	}

	rc = set_dm556_control(node_id, 0x4f);
	if (0 != rc) {
		return -1;
	}

	dm556_state = get_dm556_state(node_id);
	if(0x8237 != (dm556_state & 0x8237)){
		return -1;	
	}

	rc = set_dm556_control(node_id, 0x5f);
	if (0 != rc) {
		return -1;
	}

	dm556_state = get_dm556_state(node_id);
	//这里不知道为什么和雷赛的不一样，相应的位都是保留位没有描述,所以这里先注释掉
	/*if(0x4237 == (dm556_state & 0x4237)){
		return -1;	
	}*/	
	return 0;
}

/**
 * @brief 设置绝对位置移动
 * 
 * @param node_id CAN端口与从站节点号
 * @param target_position 绝对位置值
 * @return int32_t 0成功，非0表示失败
 */
int32_t absolute_position_move(const can_node_id_t *node_id, int32_t target_position)
{
	uint16_t dm556_state;
	int32_t rc = 0;

	rc = set_target_position(node_id, target_position);
	if (0 != rc) {
		return -1;
	}

	rc = set_dm556_control(node_id, 0x0f);
	if (0 != rc) {
		return -1;
	}

	dm556_state = get_dm556_state(node_id);
	if(0x8237 != (dm556_state & 0x8237)){
		return -1;	
	}

	rc = set_dm556_control(node_id, 0x1f);
	if (0 != rc) {
		return -1;
	}

	dm556_state = get_dm556_state(node_id);
	//这里不知道为什么和雷赛的不一样，相应的位都是保留位没有描述,所以这里先注释掉
	/*if(0x4237 == (dm556_state & 0x4237)){
		return -1;	
	}*/	
	
	return 0;
}

/**
 * @brief 快速停止，停止后需要重新切换驱动器状态机到可操作性状态
 * 
 * @param node_id CAN端口与从站节点号
 * @return int32_t 0成功，非0表示失败
 */
int32_t quick_stop(const can_node_id_t *node_id)
{
	uint16_t dm556_state;
	int32_t rc = 0;

	rc = set_dm556_control(node_id, 0x0002);
	if (0 != rc) {
		return -1;
	}

	rc = set_dm556_control(node_id, 0x0000);
	if (0 != rc) {
		return -1;
	}
	
	return 0;
}

/**
 * @brief 执行减速停止，停止时按指定减速度停止
 * 
 * @param node_id CAN端口与从站节点号
 * @param deceleration 减速停止速度，设置为0时电机无法减速停止
 * @return int32_t 0成功，非0表示失败
 */
int32_t slow_down(const can_node_id_t *node_id, uint32_t deceleration)
{
	int32_t rc = 0;

	rc = set_deceleration(node_id, deceleration);
	if (0 != rc) {
		return -1;
	}

	rc = set_dm556_control(node_id, 0x10f);
	if (0 != rc) {
		return -1;
	}
	
	return 0;
}

/**
 * @brief 清除减速停止，减速停止后执行该函数让电机继续运转
 * 
 * @param node_id CAN端口与从站节点号
 * @return int32_t 0成功，非0表示失败
 */
int32_t clear_slow_down(const can_node_id_t *node_id)
{
	int32_t rc = 0;

	rc = set_dm556_control(node_id, 0x00f);
	if (0 != rc) {
		return -1;
	}
	
	return 0;
}




/**
 * @brief 协议位置参数配置
 * 
 * @param node_id CAN端口与从站节点号
 * @param p_position_config 协议位置参数
 * @return int32_t 0成功，非0表示失败
 */
int32_t profile_position_config(const can_node_id_t *node_id, s_profile_position_config *p_position_config)
{
	int32_t rc = 0;

	rc = set_operating_mode(node_id, PROFILE_POSITION);
	if (0 != rc) {
		return -1;
	}

	if(PROFILE_POSITION != get_operating_mode(node_id)){
			return -1;
	}

	rc = set_motor_resolution(node_id, p_position_config->motor_resolution);
	if (0 != rc) {
		return -1;
	}

	rc = set_profile_velocity(node_id, p_position_config->profile_velocity);
	if (0 != rc) {
		return -1;
	}

	rc = set_end_velocity(node_id, p_position_config->end_velocity);
	if (0 != rc) {
		return -1;
	}

	rc = set_profile_acceleration(node_id, p_position_config->profile_acceleration);
	if (0 != rc) {
		return -1;
	}
	
	rc = set_profile_deceleration(node_id, p_position_config->profile_deceleration);
	if (0 != rc) {
		return -1;
	}
	
	return 0;
}

/**
 * @brief 协议速度参数配置
 * 
 * @param node_id CAN端口与从站节点号
 * @param profile_acceleration 协议加速度
 * @param profile_deceleration 协议减速度
 * @return int32_t 0成功，非0表示失败
 */
int32_t profile_velocity_config(const can_node_id_t *node_id, uint16_t motor_resolution,
											uint32_t profile_acceleration, uint32_t profile_deceleration)
{
	int32_t rc = 0;

	rc = set_operating_mode(node_id, PROFILE_VELOCITY);
	if (0 != rc) {
		return -1;
	}

	if(PROFILE_VELOCITY != get_operating_mode(node_id)){
			return -1;
	}
	
	rc = set_motor_resolution(node_id, motor_resolution);
	if (0 != rc) {
		return -1;
	}

	rc = set_profile_acceleration(node_id, profile_acceleration);
	if (0 != rc) {
		return -1;
	}
	
	rc = set_profile_deceleration(node_id, profile_deceleration);
	if (0 != rc) {
		return -1;
	}
	
	return 0;
}


/*
set fir filter 
param:
	node_id: canopen id
	state: 0=no enable,1=enable
*/
int32_t set_firEN_func(const can_node_id_t *node_id, u8 state){
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, 0x2009, 0, &state, 1);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", 0x2009, 0);
		return -1;
	}

	return 0;
}

/*
set fir filter time
param:
	node_id: canopen id
	time: fitler keep time, us
*/
int32_t set_firTime_func(const can_node_id_t *node_id, u16 time){
	int32_t rc = 0;
	
	rc = co_sdo_write_od(node_id, 0x2010, 01, &time, 2);
	if (0 != rc) {
		LOGE(CANOPEN_MASTER_LOG_TAG, "OD Write on 0x%x 0x%x fail", 0x2010, 01);
		return -1;
	}

	return 0;
}